//#include "xinput.h"
#define WITH_JOYSTICKS
#include "WPILib.h"
#include "math.h"
#include "DashboardDataSender.h"
#ifndef WITH_JOYSTICKS
//#include "CXBoxController.h"
#endif
#define MINIMUM_SCORE 0.01


void CheckSolenoid(void);
#ifdef WITH_JOYSTICKS
void OmniDrive(GenericHID*, GenericHID*);
#else
void OmniDrive(CXBOXController*);
#endif
/*void OmniDrive(float, float, float, float);
float Limit(float);*/
/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
#ifdef WITH_JOYSTICKS
	Joystick *stick, *stick2; // only joystick DISREGARD THAT I SUCK COCKS!!!!!!!!!
#else
	CXBOXController *xboxController;
#endif
	Solenoid *firingMechanism;
	Compressor *compressor;
	DashboardDataSender *dds;

public:
	RobotDemo(void): 
		myRobot(1, 3, 2, 4)	// these must be initialized in the same order
		{
			GetWatchdog().SetExpiration(0.1);
#ifdef WITH_JOYSTICKS
			stick = new Joystick(1);				// as they are declared above
			stick2 = new Joystick(2);
#else
			xboxController = new CXBOXController(1);
#endif
			firingMechanism = new Solenoid(1);
			compressor = new Compressor(1, 6);
			dds = new DashboardDataSender();
		}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
		while(IsAutonomous())
		{
			DoCamera();
		}
	}

	bool firingEnabled;
	Timer timer;
	/**
	 * Runs the awesome wheels steering and does checks on the solenoid
	 * firing and compressor
	 */
	void OperatorControl(void)
	{
		myRobot.SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
		myRobot.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
		AxisCamera &camera = AxisCamera::GetInstance();
		camera.WriteResolution(AxisCamera::kResolution_320x240);
		camera.WriteCompression(20);
		camera.WriteBrightness(0);
		firingEnabled = FALSE;
		timer.Start();
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
#ifdef WITH_JOYSTICKS
			OmniDrive(stick, stick2);
#else
			OmniDrive(xboxController);
#endif
			CheckSolenoid();
			CheckCompressor();
			//GetWatchdog().Feed();
			Wait(0.005);				// wait for a motor update time
		}
	}
	
	void DoCamera()
	{
		AxisCamera &camera = AxisCamera::GetInstance();
		
		if (camera.IsFreshImage())
		{
			timer.Reset();
			// get the camera image
			HSLImage *image = camera.GetImage();
			

			// find FRC targets in the image
			vector<Target> targets = Target::FindCircularTargets(image);
			delete image;
			if (targets.size() == 0 || targets[0].m_score < MINIMUM_SCORE)
			{
				// no targets found. Make sure the first one in the list is 0,0
				// since the dashboard program annotates the first target in green
				// and the others in magenta. With no qualified targets, they'll all
				// be magenta.
				Target nullTarget;
				nullTarget.m_majorRadius = 0.0;
				nullTarget.m_minorRadius = 0.0;
				nullTarget.m_score = 0.0;
				if (targets.size() == 0)
					targets.push_back(nullTarget);
				else
					targets.insert(targets.begin(), nullTarget);
#ifdef WITH_JOYSTICKS
				dds->sendVisionData(stick->GetY(), 0.0, 0.0, 0.0, targets);
#else
				dds->sendVisionData(((float)xboxController->GetState().Gamepad.sThumbLY)/32768,
										0.0, 0.0, 0.0, targets);
#endif
			}
			else {
				// We have some targets.
				// set the new PID heading setpoint to the first target in the list
				//double horizontalAngle = targets[0].GetHorizontalAngle();
				//double setPoint = horizontalAngle;

				//turnController.SetSetpoint(setPoint);
				
				// send dashbaord data for target tracking
				dds->sendVisionData(0.0, 0.0, 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);
			}
			printf("Time: %f\n", 1.0 / timer.Get());
		}
	}
	
	/**
	 * Checks the appropriate controller to see if the solenoid should be fired
	 * if so takes action accordingly
	 */
	void CheckSolenoid()
	{
#ifdef WITH_JOYSTICKS
		if (stick == NULL || stick2 == NULL)
		{
			wpi_fatal(NullParameter);
			return;
		}
		if (stick->GetRawButton(3))
		{
			firingEnabled = !firingEnabled;
			while(stick->GetRawButton(3))
			{ }
		}
		if (firingEnabled && stick->GetRawButton(1))
			firingMechanism->Set(true);
		else
			firingMechanism->Set(false);
#else
		if (xboxController->IsConnected())
		{
			//max value is 255
			if (xboxController->GetState().Gamepad.bLeftTrigger > 200 && 
					xboxController->GetState().Gamepad.bRightTrigger > 200)
				firingMechanism->Set(true);
		}
#endif
	}
	
	/**
	 * Checks the comressor switch and starts it when enabled
	 */
	void CheckCompressor()
	{
		UINT32 valve = compressor->GetPressureSwitchValue();	
		if (valve)
			compressor->Stop();
		else
			compressor->Start();
	}
	
	/**
	 * Because mecanum wheels are that awesome, this takes input and 
	 * drives the robot accordingly
	 */
#ifdef WITH_JOYSTICKS
	void OmniDrive(GenericHID *leftStick, GenericHID *rightStick)
	{
		if (leftStick == NULL || rightStick == NULL)
		{
			wpi_fatal(NullParameter);
			return;
		}
		float leftYValue = -leftStick->GetY();
		float leftXValue = leftStick->GetX();
		float magnitude = sqrt((leftYValue * leftYValue) + (leftXValue * leftXValue));
		if (magnitude < .1)
			magnitude = 0;
		if (leftXValue > -.1 && leftXValue < .1)
			leftXValue = .00001;
		if (leftYValue > -.1 && leftYValue < .1)
			leftYValue = .00001;
		float direction = (180 / 3.14159) * atan(leftXValue/leftYValue);
		//if in the lower quadrants...
		if (leftYValue < 0.0)
			direction += 180.0;
		//rotation is based on the second joystick
		float rotation = rightStick->GetX();
		if (rotation < .1 && rotation > -.1)
			rotation = 0;
		myRobot.HolonomicDrive(magnitude, direction,  rotation);
		
	}
#else
	void OmniDrive(CXBOXController *controller)
	{
		if (controller->IsConnected())
		{
			XINPUT_GAMEPAD gamepad = controller->GetState().Gamepad;
			float leftYValue = 0, leftXValue = 0, rotation = 0;
			if (gamepad.sThumbLX > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
				leftXValue = ((float)gamepad.sThumbLX)/32768.0;
			if (gamepad.sThumbLY > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
				leftYValue = ((float)gamepad.sThumbLY)/32768.0;
			if (gamepad.sThumbRX > XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE)
				rotation = ((float)gamepad.sThumbRX)/32768.0;
			//reuse code :D
			float magnitude = sqrt((leftYValue * leftYValue) + (leftXValue * leftXValue));
			//if its 0 then we dont want to divide by 0 so set it to be extremely small
			if (leftXValue == 0.0)
				leftXValue = .00001;
			if (leftYValue == 0.0)
				leftYValue = .00001;
			//direction is determined by inverse cot
			//yay maths. forgetting to switch between rad and deg is bad :|
			float direction = (180 / 3.14159) * atan(leftXValue/leftYValue);
			//if in the lower quadrants...
			if (leftYValue < 0.0)
				direction += 180.0;
			//rotation is based on the second joystick
			myRobot.HolonomicDrive(magnitude, direction,  rotation);
		}
	}
#endif
};

START_ROBOT_CLASS(RobotDemo);

